/*Incremental_PID*/

#include "include.h"
static void INCParamInit(
       INCPID_t *pid,
       float p,
       float i,
       float d,
       float max_output)
{
    pid->p = p; 
    pid->i = i; 
    pid->d = d;
    pid->max_output = max_output; 
}

static void INCReset(
        INCPID_t *pid,
        float p,
        float i,
        float d,
        float max_output)
{ 
    pid->p = p; 
    pid->i = i; 
    pid->d = d; 
    pid->max_output = max_output; 
    pid->last_err = 0;
    pid->last_last_err = 0;
    pid->target_value = 0;
    pid->current_value = 0;
}

static void INCCalculate(
        INCPID_t *pid,
        float current_value,float target_value)
{
    pid->current_value = current_value;
    pid->target_value = target_value;
    pid->last_last_err = pid->last_err;
    pid->last_err = pid->err;
    pid->err = pid->target_value - pid->current_value;
    
    float p_value = pid->p * (pid->err - pid->last_err); 
    float i_value = pid->i * pid->err;
    float d_value = pid->d * (pid->last_err - pid->last_last_err);
    
    pid->output += p_value + d_value + i_value;
    if (pid->output > pid->max_output)
    {
        pid->output = pid->max_output;
    }
    if (pid->output < -pid->max_output)
    {
        pid->output = -pid->max_output;
    }
}

void INCPID_StructInit(INCPID_t *pid)
{
    pid->f_ParamInit = INCParamInit;
    pid->f_Reset = INCReset;
    pid->f_Calculate = INCCalculate;
    pid->last_err = 0;
    pid->last_last_err = 0;
    pid->target_value = 0;
    pid->current_value = 0;
}

